#include "bits/stdc++.h"
#include "ros/ros.h"
#include "unistd.h"
#include <sensor_msgs/LaserScan.h>

using namespace std;
#define BIZHANG 6
#define MINDIST 0.8

#define div 2

int state; // flag: 0=find obstacle ;1=move around obstacle
int flag = 0;

int cnt = 0;

float findMinDistance(vector<float> ranges) {
    float mindistance = ranges[175];
    for (int i = 210; i < 240; i++) {
        if (ranges[i] < mindistance) {
            mindistance = ranges[i];
        }
    }
    return mindistance;
}

void laserCallback(const sensor_msgs::LaserScan::ConstPtr &msg) {
    ros::param::get("state", state);

    if (findMinDistance(msg->ranges) > MINDIST && state != BIZHANG) {
        return;
    }
    else if (findMinDistance(msg->ranges) <= MINDIST && state != BIZHANG) {
        int state_last = state;
        ros::param::set("state", BIZHANG);

        cout << "\033[44mfind obstacle\033[0m" << endl;
        ros::param::set("gear", 1);
        for (int i = 0; i < 200 / div; i++) {
            ros::param::set("state", BIZHANG);
            usleep(10000);
        }
        //        ros::param::set("gear", 2);
        //        for ( int i = 0; i < 150 / div; i++ ) {   // 斜300
        //            ros::param::set("state", BIZHANG);
        //            usleep(10000);
        //        }

        ros::param::set("gear", 5);
        for (int i = 0; i < 450 / div; i++) { // 斜700
            ros::param::set("state", BIZHANG);
            usleep(10000);
        }
        ros::param::set("gear", 2);
        for (int i = 0; i < 150 / div; i++) { // 斜75
            ros::param::set("state", BIZHANG);
            usleep(10000);
        }
        cout << "\033[44mavoid over\033[0m" << endl;
        ros::param::set("state", state_last);
    }
    flag = 1;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "avoid");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/scan", 1, laserCallback);
    ros::Rate looprate(100);
    int light = 0;
    //    while(light == 0)
    //    {
    //        ros::param::get("light", light);
    //        usleep(10000);
    //    }
    //    sleep(10);
    int stim_last = 1;
    int stim_cur = 1;
    cnt = 0;

    while (ros::ok()) {
        ros::param::get("stim", stim_cur);
//    if (stim_last == 1 && stim_cur != 1) {
//      cnt++;
//    }
//    if (cnt < 1) {
//      looprate.sleep();
//      continue;
//    }
//    else if(cnt == 1){
//      sleep(10);
//      cnt++;
//    }
        ros::param::get("light", light);
        if (light == 1 && cnt == 0) {
            sleep(5);
            cnt++;
        }

        if (flag == 0 && cnt == 1) {
            ros::spinOnce();
        }

        else if(flag == 1){
            break;
        }
        looprate.sleep();
    }
    return 0;
}
